//
// Created by oftenlin on 23-9-11.
//

#ifndef LANGGE_VDR_NAVIDATA_H
#define LANGGE_VDR_NAVIDATA_H
#include "Eigen/Core"
#include "Eigen/Geometry"
#include <vector>
#include "simpleRtk.hpp"
#include "loc_signal_type.h"
#include <memory>
namespace NaviData {
    enum TIMETYPE {
        UNKONWTYPE = -1,
        GPSTIME,
        MCUTIME
    };

    enum GPSTIMETYPE {
        WEEK_TOW,
        SECONDS,
        UNKNOWN
    };

    struct GPSTIME_T {
        gtime_t gtime;
        int week;
        double tow;
    };

    enum DataType{
        DATAUNKOWN = -1,
        IMUDATA,
        GNSSDATA,
        CANVELOCITYDATA,
        CAMERADATA,
        RESULTDATA,
        SDMMDATA    //sd-MM 投影信息
    };

    class NavTime {
    public:
        NavTime();

        NavTime(const gtime_t &);

        NavTime(const double mcu_time_);

        //NavTime(int week, double tow);

        NavTime(const NavTime &);

        NavTime operator+(double second);

        NavTime operator-(double second);

        NavTime &operator=(const NavTime &init_navtime);

        double operator-(const NavTime &time) const;

        bool operator<(const NavTime &time) const;

        bool operator>(const NavTime &time) const;

        bool operator==(const NavTime &time) const;

        void setNavtime(double mcu_time);

        void setNavtime(const gtime_t &gtime);

        void setZero();

        friend std::ostream &operator<<(std::ostream &output, const NavTime &nav_time);

        std::string getTimeStr() const;

        const TIMETYPE getTimeType();

        double getTimeStamps() const;

        bool isZero();

        friend std::vector<NavTime> getFixefStepTime(const NavTime &t0, const NavTime &t1, double stepsize);

        ~NavTime() = default;
        //gtime_t gpst2time(int week, double sec);
    private:
        double mcu_time_;
        GPSTIME_T gpstime_;
        TIMETYPE time_type_;
    };
    class BaseData{
    public:
        BaseData(const NavTime &time,DataType init_data_type);
        BaseData();
        virtual ~BaseData() = default;
        using bPtr = std::shared_ptr<BaseData>;

        virtual NavTime get_time() const;
        void set_time(const NavTime &time);
        DataType get_type() const;
    private:
        DataType data_type_ = DATAUNKOWN;
        NavTime t0_;
    };

    class GnssData : public BaseData{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        struct GNSSOBSDetail{
            Eigen::Vector3d pos_{0, 0, 0};//blh
            double mSpeed{0};
            double mUtcTickTime{0.0};
            LOC_TYPE mLocType{LOC_TYPE::LOC_TYPE_UNKNOWN};
            double mHeading{0.0};
            double mHeight{0.0};
            double mAccuracyVelocity{0.0};
            double mAccuracyLocHorizon{0.0};
            double mAccuracyLocVertical{0.0};
            double mAccuracyHeadingAngular{0.0};
            int sv_nums{0};
        };
        using Ptr = std::shared_ptr<GnssData>;
        GnssData(const NavTime &time,GNSSOBSDetail& init_gnssobs_detail);
        GnssData();
        const GNSSOBSDetail& getGnssObsDetail();
        //void setGnssData(const NavTime& cur_navtime, const GNSSOBSDetail& cur_gnssobs_detail);
        //void setGnssData(NavTime&& cur_navtime, GNSSOBSDetail&& cur_gnssobs_detail);
        NavTime get_time() const override;
        virtual ~GnssData() = default;
    private:
        GNSSOBSDetail gnss_obs_detail_;
    };
    //using GNSSDATAPOOL = std::deque<GnssData::Ptr>;


    class ImuData : public BaseData{
    public:
        struct ImuDataDetail{
            Eigen::Vector3d gyro_{0, 0, 0};//陀螺角速度
            Eigen::Vector3d acce_{0, 0, 0};//加计加速度
            Eigen::Vector3d dtheta{0, 0, 0};//角增量
            Eigen::Vector3d dvel{0, 0, 0};//速度增量
            double dt=0.0;//the delta time between current epoch and previous epoch
            bool resetAlign = false; //如果imu的前后历元的时间差
            ImuDataDetail& operator=(const ImuDataDetail& input);
        };
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        using Ptr = std::shared_ptr<ImuData>;
        ImuData(const ImuData& in_imudata);
        ImuData(const NavTime &time, const ImuDataDetail& init_imudata_detail);
        ImuData();
        void setImuData(const NavTime& cur_navtime, const ImuDataDetail& cur_imudata_detail);
        NavTime get_time() const;
        virtual ~ImuData() = default;
        const ImuDataDetail& getImuDataDetail();
        ImuData splitData(const NavTime& cur_navtime);
    private:
        ImuDataDetail imudata_detail_;
    };
    //using IMUDATAPOOL = std::deque<ImuData::Ptr>;

    class CANVelocity:public BaseData{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        struct CANVelocityDetail{
            double velocity{0.0};//车速 m/s
        };
        using Ptr = std::shared_ptr<CANVelocity>;
        CANVelocity(const NavTime& time);
        CANVelocity();
        ~CANVelocity() override =default;
        NavTime get_time() const override;
        void setCanVelocity(const NavTime& cur_navtime, const CANVelocityDetail& cur_canVelocity_detail);
        const CANVelocityDetail& getCanVelocityDetail();
    private:
        CANVelocityDetail CanVelocity_detail;
    };

    class CameraData : public BaseData{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        struct CameraDataDetail{
            double mcu_time{0.0};   // unit: second, resoluton: 0.001s
            Eigen::Vector3d pos_{0.0,0.0,0.0};//lat(rad),lon(rad),meter
            Eigen::Vector3d att_{0.0,0.0,0.0};//roll,pitch,yaw, unit:rad
            double speed{0.0};//m/s
            double score{0.0};
            double overlap{0.0};
            double std_x{0.0};
            double std_y{0.0};
            double std_yaw{0.0};
            double confidence{0.0};
            double alignability{0.0};
            double dis_between_gnss{0.0};
            bool has_attitude{false};
            bool has_spd{false};
            bool gps_lost{false};
            bool is_valid{false};
            bool is_aligned_gnss{false};
            int errorType{2}; // 0-no,1-proto,2-errorcode
        };
        using Ptr = std::shared_ptr<CameraData>;
        CameraData(const NavTime &time);
        CameraData(const CameraData &camera);
        CameraData &operator=(const CameraData &camera);
        NavTime get_time() const;
        ~CameraData() {}
    private:
        CameraDataDetail camera_data_detail_;
    };

    class linkInfoGcj02 {
    public:
        std::int64_t plan_linkId = -1;
        double plan_proj_lon = 0;
        double plan_proj_lat = 0;
        float plan_direction = 0;
        double tunnel_distance = -1;
        double traveled_tunnel_distance = -1;
        bool isTunnel = false;
        bool canUse2Fuse = false;
        std::int64_t timestamp = -1;
        float accuracy = 3.3f;

        linkInfoGcj02() = default;
        const bool isValid() {
            if (plan_linkId > 0 && plan_direction != 0) {
                return true;
            } else {
                return false;
            }
        }
    };
    class LinkData : public BaseData{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        struct LinkDataDetail{
            Eigen::Vector3d pos_{0, 0, 0};//llh_rad
            double direction{-1}; //  (0,360]
            bool isTunnel{false};
            bool canUse{false};
        };
        NavTime get_time() const override;
        LinkData(linkInfoGcj02 &curLinkInfo);
        LinkData();
        using Ptr = std::shared_ptr<LinkData>;
        //void setLinkData(const NavTime& cur_navtime, const LinkDataDetail& curLinkDataDetail);
        const LinkDataDetail& getLinkDataDetail();
    private:
        LinkDataDetail mLinkDetail_;

    };

    struct NavInfo {
        NavInfo(const gtime_t &gtime);

        gtime_t time_;
        Eigen::Vector3d pos_{0, 0, 0};//纬度（rad） 经度(rad) 高程(m)
        Eigen::Vector3d vel_{0, 0, 0};// N E D
        Eigen::Vector3d att_{0, 0, 0};// roll pitch heading
        Eigen::Quaterniond quat_{1, 0, 0, 0};
        Eigen::Matrix3d rotation_ = Eigen::Matrix3d::Identity();//DCM=euler2DCM=Cbn

        Eigen::Vector3d cbv_att_{0, 0, 0};
        Eigen::Matrix3d cbv_dcm = Eigen::Matrix3d::Identity();
        Eigen::Quaterniond cbv_quat_{1, 0, 0, 0};//安装角cbv
        Eigen::Vector3d acce_bias_{0, 0, 0};
        Eigen::Vector3d gyro_bias_{0, 0, 0};
        Eigen::Vector3d gyro_scale_{0, 0, 0};//一般不估计
        Eigen::Vector3d acce_scale_{1.0, 1.0, 1.0};//一般不估计
        // 增加内部收敛状态监测
        NavInfo() = default;

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };


}
#endif //LANGGE_VDR_NAVIDATA_H
